import time
import cv2
from driver_server import VSRController
# from driver_server import IOACamera

import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp
import socket

### 四轴机械臂点位
## 1开头--2待取料--3取料--4抬高--5待放--6放料--7货叉出仓--8抬高
P1=[0.0,10.45,62.05,0.0,0.0,0.0,146.011,0.0,143.008,0.0,0.0,0.0,0.0]
P2=[81.1, 65.3, 86.3, 8.1, 0.0, 0.0, 29.5, 189.0, 47.7, 8.1, 0.0, 0.0, 0.0]

P3=[82.9, 69.9, 67.4, 8.1, 0.0, 0.0, 29.5, 238.5, 47.7, 8.1, 0.0, 0.0, 0.0]
P4=[82.9, 63.8, 64.7, 8.1, 0.0, 0.0, 29.5, 238.5, 64.2, 8.1, 0.0, 0.0, 0.0]
P5=[0.0,0.0,0.0,0.0,0.0,0.0,198.67,0.0,230.72,0.0,0.0,0.0,0.0]

P6=[-24.1, 71.6, 80.1, 112.1, 0.0, 0.0, 193.1, -87.4, 35.3, 112.1, 0.0, 0.0, 0.0]
P7=[-47.9, 74.4, 69.8, 125, 0.0, 0.0, 159.8, -176.7, 35.3, 125, 0.0, 0.0, 0.0]
P8=[-50.6, 52.7, 32.7, 132.2, 0.0, 0.0, 180.5, -219.6, 138.3, 132.2, 0.0, 0.0, 0.0]

### 六轴机械臂点位
## 1号仓位待取--取料--待放料--放料
P9=[-37.5, 15.9, -5.1, 3.4, -11.8, -1.0, 182.9, -138.5, 194.6, 3.4, -0.9, -39.2, 0.0]
P10=[-38, 22.1, 1.7, 4.8, -25.7, -1.6, 185.7,-142.5, 156.3, 2.7, 3.2, -39.2, 0.0]
P11=[-6.0, 38.6, 0.3, 0.0, -39.8, 17.7, 240.3, 25.2, 96.6, -0.3, -0.9, 11.7, 0.0 ]
P12=[-6.0, 30.5, -0.5, 0.2, -32.7, 21, 240.3, 128.2, 96.6, -0.3, -0.9, 11.7, 0.0 ]

## 过渡点
P13=[-29.175,0.0,0.0,0.0,0.0,0.0,173.466,-96.846,230.72,0.0,0.0,-29.175,0.0]

## 2号仓位 待取--取料--待放料--放料
P14=[-49.8, 29.8, -16.3, 2, -4.7, 2.1, 161.2, -189.3, 176.7, 2.3, 8.8, -47.8, 0.0]
P15=[-49.8, 31.9, -11.7, 2.1, -11.3, 2.4, 161.2, -189.3, 155.1, 2.3, 8.8, -47.8, 0.0]
P16=[-2.6, 56.1, -31.4, 0.0, -24.8, 24.5, 280.9 -12.9, 110.2, 0, 0, 21.9, 0.0]
P17=[-2.6, 60, -31.4, 0.0, -28.6, 24.5, 280.9 -12.9, 93.2, 0, 0, 21.9, 0.0]

## 3号仓位 待取--取料--待放料--放料
P18=[-60.3, 26.6, -12.0, 0.4, -5.4, 12.4, 120.4, -210.8, 176.7, 2.3, 8.8, -47.8, 0.0]
P19=[-61.1, 27, -4.9, 0.3, -13, 13.2, 115.6, -209, 154.3, 2.3, 8.8, -47.8, 0.0]
P20=[-10.7, -75, 63.0, 5.3, -16.3, 19.6, 300.4, -54.7, 115.5, 3.5, -5.8, 7.4, 0.0]
P21=[-9.5, -80.5, 63.4, 5.3, -16.9, 20.7, 299.8, -47.9, 90.3, 4.8, -1.6, 9.6, 0.0]

## 4号仓位 待取--取料--待放料--放料
P22=[-72.7, 19.3, 0.6, -1.6, -10.9, 24.4, 66.8, -217.2, 166.3, 2.3, 8.8, -47.8, 0.0]
P23=[-72.7, 21.3, 3.1, -1.6, -15.5, 24.2, 66.8, -217.2, 151.9, 2.3, 8.8, -47.8, 0.0]
P24=[-14.3, 47.8, -19.7, 0.0, -28.1, 36.2, 259.9, -66.3, 113.4, 0.0, 0.0, 15.6, 21.9]
P25=[-14.4, 52.3, -19.7, 0.0, -32.7, 36.2, 259.9, -66.3, 94.4, 0.0, 0.0, 15.6, 21.9]


def run_demo():
    try:
        controller.connect()
        ## 示例机械臂运动指令
        # controller.set_joint_angle(com_Pose= P1, com_Num=1)

        ## 示例：控制输出接口
        # controller.SetDO(0, 1)
        # time.sleep(2)
        # controller.SetDO(0, 0)

        ## 六轴机械臂：取料到货架--机械臂动作流程
        getGoods()

        ## 四轴机械臂：货叉--插取物料托盘到传送带
        putGoodsPlate()

        ### 颜色识别
        # process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 1)
        ### 形状识别
        # process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 2)
        ### QR 识别
        process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 3)
        ### OCR
        # process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 4)

        process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 0)


    except Exception as e:
        print(e)
    
    finally:
        if controller._running:
            controller.stop()

##六轴机械臂：取料到货架--机械臂动作流程
def getGoods():
    ### 取1号仓位物料--放托盘
    controller.set_joint_angle(com_Pose= P9, com_Num=1)
    controller.set_joint_angle(com_Pose= P10, com_Num=1)
    controller.SetDO(0,1)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P9, com_Num=1)
    # controller.set_joint_angle(com_Pose= P13, com_Num=1)
    controller.set_joint_angle(com_Pose= P11, com_Num=1)
    controller.set_joint_angle(com_Pose= P12, com_Num=1)
    controller.SetDO(0,0)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P11, com_Num=1)
    controller.set_joint_angle(com_Pose= P13, com_Num=1)


    ### 取2号仓位物料--放托盘
    controller.set_joint_angle(com_Pose= P14, com_Num=1)
    controller.set_joint_angle(com_Pose= P15, com_Num=1)
    controller.SetDO(0,1)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P14, com_Num=1)
    # controller.set_joint_angle(com_Pose= P13, com_Num=1)
    controller.set_joint_angle(com_Pose= P16, com_Num=1)
    controller.set_joint_angle(com_Pose= P17, com_Num=1)
    controller.SetDO(0,0)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P16, com_Num=1)
    controller.set_joint_angle(com_Pose= P13, com_Num=1)

    ### 取3号仓位物料--放托盘
    controller.set_joint_angle(com_Pose= P18, com_Num=1)
    controller.set_joint_angle(com_Pose= P19, com_Num=1)
    controller.SetDO(0,1)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P18, com_Num=1)
    # controller.set_joint_angle(com_Pose= P13, com_Num=1)
    controller.set_joint_angle(com_Pose= P20, com_Num=1)
    controller.set_joint_angle(com_Pose= P21, com_Num=1)
    controller.SetDO(0,0)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P20, com_Num=1)
    controller.set_joint_angle(com_Pose= P13, com_Num=1)

    ### 取4号仓位物料--放托盘
    controller.set_joint_angle(com_Pose= P22, com_Num=1)
    controller.set_joint_angle(com_Pose= P23, com_Num=1)
    controller.SetDO(0,1)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P22, com_Num=1)
    # controller.set_joint_angle(com_Pose= P13, com_Num=1)
    controller.set_joint_angle(com_Pose= P24, com_Num=1)
    controller.set_joint_angle(com_Pose= P25, com_Num=1)
    controller.SetDO(0,0)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P24, com_Num=1)
    controller.set_joint_angle(com_Pose= P13, com_Num=1)

    time.sleep(1)

## 四轴机械臂：货叉--插取物料托盘到传送带
def putGoodsPlate():
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P1, com_Num=2)
    controller.set_joint_angle(com_Pose= P2, com_Num=2)
    controller.set_joint_angle(com_Pose= P3, com_Num=2)
    controller.set_joint_angle(com_Pose= P4, com_Num=2)
    controller.set_joint_angle(com_Pose= P5, com_Num=2)
    controller.set_joint_angle(com_Pose= P6, com_Num=2)
    controller.set_joint_angle(com_Pose= P7, com_Num=2)
    controller.set_joint_angle(com_Pose= P8, com_Num=2)
    time.sleep(1)
    controller.SetDO(1,1)
    time.sleep(6)
    controller.SetDO(1,0)
    time.sleep(1)



class process_Master():
    def __init__(self):
        super().__init__()
        self.host = socket.gethostname()
        self._master_init()

    def _master_init(self):
        self.master_ = modbus_tcp.TcpMaster(host=self.host)


if __name__ == "__main__":
    controller = VSRController(simulate=True)
    process_master = process_Master()
    run_demo()

